// Devin Koepl


#ifndef FUNCS_H_CONTROLLER
#define FUNCS_H_CONTROLLER

#include <drl_library/drl_math.h>

#ifdef COMPILE_FOR_RTAI
 #include <linux/types.h>
 #include <rtai_math.h>
#else
 #include <stdint.h>
 #include <math.h>
#endif


#define SIZE_OF_CONTROLLER_DATA 		100
#define SIZE_OF_CONTROLLER_STATE_DATA	100

//======================================================//

// Debugging print statements.

//#define DEBUG_CONTROLLERS
#undef	DEBUG_CONTROLLERS


#ifdef DEBUG_CONTROLLERS
	#ifdef COMPILE_FOR_RTAI
			//#include <rtai_sem.h>
			#define PRINT_MSG	rt_printk
			#define PRINT_WARN	rt_printk
			//#define PRINT_MSG	rtai_print_to_screen
			//#define PRINT_WARN	rtai_print_to_screen
		#else
			#include <ros/ros.h>
			#define PRINT_MSG	ROS_INFO
			#define PRINT_WARN	ROS_WARN
	#endif
#else
	#define PRINT_MSG	// printf
	#define PRINT_WARN	// printf
#endif

// Temporary for testing in uspace.
//#include <rtai_lxrt.h>

//#define PRINT_MSG rtai_print_to_screen
//#define PRINT_WARN rtai_print_to_screen

//======================================================//

// Controller types available.

#define NO_CONTROLLER 					0
#define MOTOR_TORQUE_CONTROLLER 		1
#define MOTOR_POSITION_CONTROLLER 		2
#define FORCE_CONTROLLER				3
#define TORQUE_SINE_CONTROLLER			4
#define POSITION_SINE_CONTROLLER		5
#define OPENLOOP_SINE_CONTROLLER		6

//======================================================//

// If the controller commands both motors to have a torque below this value, assume that no controller is present,
// and command a small torque to keep the robot off of its hardstops.
#define MIN_TRQ_THRESH					1E-9

////////////////////////////////////////////////////////////////////////////////

// General control structs.

typedef struct
{
	int				motor_cnt;
	int				leg_cnt;
	
	float			motor_angle;
	float			leg_angle;

	float			motor_velocity;
	float			leg_velocity;

	unsigned char	motor_current;

	unsigned char	command;
} ControllerInput;

typedef struct
{
	float			motor_torque;
	float			time;
} ControllerOutput;

// This struct is where the controller can keep personal information.
typedef struct
{
	unsigned char 	state;
	unsigned char 	controller_loaded;

	// Controller specific space.
	unsigned char 	data[SIZE_OF_CONTROLLER_STATE_DATA];
} ControllerState;

// This struct is the input to the controller.
typedef struct
{
  unsigned char command;
  unsigned char controller_requested;

  // Controller specific space.
  unsigned char data[SIZE_OF_CONTROLLER_DATA];
} ControllerData;

////////////////////////////////////////////////////////////////////////////////
// Specific control structs

typedef struct
{
	float mtr_trq;
} MtrTrqControllerData;

typedef struct {
	float time;
} MtrTrqControllerState;

//////////////////////////////////

typedef struct 
{
	float mtr_ang;
	float p_gain;
	float d_gain;
} MtrPosControllerData;

typedef struct {
	float time;
} MtrPosControllerState;

//////////////////////////////////
typedef struct {
	float torque_cmd;
	float pos_cmd;
	float tp_ratio;
	float trq_lim;
	float torque_p_gain;
	float torque_d_gain;
	float torque_f_gain;
	float pos_p_gain;
	float pos_d_gain;
} ForceControllerData;

typedef struct {
	float time;
	float trq_m;
	float xl2_last;
	float xm2_ave;
	float xl2_ave;
	float xl3_ave;
} ForceControllerState;

//////////////////////////////////

typedef struct {
	float trq_amp;
	float trq_freq;
	float trq_off;
	float trq_k1;
	float trq_k2;
	float trq_ke1;
	float trq_ke2;
	float trq_lim;
} TorqueSineControllerData;

typedef struct {
	float time;
} TorqueSineControllerState;

//////////////////////////////////

typedef struct {
	float pos_amp;
	float pos_freq;
	float pos_off;
	float pos_k1;
	float pos_k2;
	float pos_ke1;
	float pos_ke2;
} PositionSineControllerData;

typedef struct {
	float time;
} PositionSineControllerState;

//////////////////////////////////

typedef struct {
	float open_k;
	float open_B;
	float open_I;
	float open_f;
	float open_a;
	float open_lim;
} OpenloopSineControllerData;

typedef struct {
	float time;
} OpenloopSineControllerState;

//////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
// Macros for dereferencing pointers.

// Controller Data pointers
#define MTR_TRQ_CONTROLLER_DATA(CONTROLLER_DATA_PTR)			((MtrTrqControllerData *)(&(CONTROLLER_DATA_PTR->data)))
#define MTR_POS_CONTROLLER_DATA(CONTROLLER_DATA_PTR)			((MtrPosControllerData *)(&(CONTROLLER_DATA_PTR->data)))
#define FORCE_CONTROLLER_DATA(CONTROLLER_DATA_PTR)				((ForceControllerData *)(&(CONTROLLER_DATA_PTR->data)))
#define TORQUE_SINE_CONTROLLER_DATA(CONTROLLER_DATA_PTR)		((TorqueSineControllerData *)(&(CONTROLLER_DATA_PTR->data)))
#define POSITION_SINE_CONTROLLER_DATA(CONTROLLER_DATA_PTR)		((PositionSineControllerData *)(&(CONTROLLER_DATA_PTR->data)))
#define OPENLOOP_SINE_CONTROLLER_DATA(CONTROLLER_DATA_PTR)		((OpenloopSineControllerData *)(&(CONTROLLER_DATA_PTR->data)))

// Controller struct pointers
#define MTR_TRQ_CONTROLLER_STATE(CONTROLLER_STATE_PTR)			((MtrTrqControllerState *)(&(CONTROLLER_STATE_PTR->data)))
#define MTR_POS_CONTROLLER_STATE(CONTROLLER_STATE_PTR)			((MtrPosControllerState *)(&(CONTROLLER_STATE_PTR->data)))
#define FORCE_CONTROLLER_STATE(CONTROLLER_STATE_PTR)			((ForceControllerState *)(&(CONTROLLER_STATE_PTR->data)))
#define TORQUE_SINE_CONTROLLER_STATE(CONTROLLER_STATE_PTR)		((TorqueSineControllerState *)(&(CONTROLLER_STATE_PTR->data)))
#define POSITION_SINE_CONTROLLER_STATE(CONTROLLER_STATE_PTR)	((PositionSineControllerState *)(&(CONTROLLER_STATE_PTR->data)))
#define OPENLOOP_SINE_CONTROLLER_STATE(CONTROLLER_STATE_PTR)	((OpenloopSineControllerState *)(&(CONTROLLER_STATE_PTR->data)))

#endif // FUNCS_H_CONTROLLER
